
#include "Camera.hpp"
#include "../Tools/Logger.hpp"
namespace Vision
{
	///////////////////////////////////////////////////////////////////
	/// Constructeur
	///////////////////////////////////////////////////////////////////
	Camera::Camera( std::string identifier, int cameraIndex ) throw(Tools::InvalidDevice):
	cameraIdentifier(identifier),cameraDevice(NULL)
	{		
		cameraDevice = cvCaptureFromCAM( cameraIndex );				
		if(cameraDevice == NULL)
		{						
			throw Tools::InvalidDevice("Echec de la construction de la camera ");			
		}
	}
	
	///////////////////////////////////////////////////////////////////
	/// Constructeur
	///////////////////////////////////////////////////////////////////
	Camera::Camera( std::string identifier ):
	cameraIdentifier(identifier), cameraDevice(NULL)
	{
		
	}

	///////////////////////////////////////////////////////////////////
	/// Destructeur
	///////////////////////////////////////////////////////////////////
	Camera::~Camera()
	{			
		Disconnect();
	}
	
	///////////////////////////////////////////////////////////////////
	// Désactive le lien avec la caméra
	///////////////////////////////////////////////////////////////////
	void Camera::Disconnect()
	{			
		if( cameraDevice != NULL)
		{			
			
			cvReleaseCapture( &cameraDevice ); 
			cameraDevice = NULL;
		}
	}
	
	///////////////////////////////////////////////////////////////////
	// Retourne l'idenfiant de la caméra
	///////////////////////////////////////////////////////////////////
	std::string Camera::GetIdentifier()
	{				
		return cameraIdentifier;
	}
	
	///////////////////////////////////////////////////////////////////
	// Prise d'une image depuis la camera
	///////////////////////////////////////////////////////////////////
	IplImage* Camera::CapturePicture()
	{		
		if( cameraDevice != NULL )
		{		
			return cvQueryFrame( cameraDevice );
		}
		else
		{
			return NULL;
		}
	}
	
	///////////////////////////////////////////////////////////////////
	// Affecte l'identifiant de la caméra
	///////////////////////////////////////////////////////////////////
	void Camera::SetIdentifier(const std::string & identifier )
	{
		cameraIdentifier = identifier;
	}
		
	///////////////////////////////////////////////////////////////////
	// Affecte les paramètres de calibrage de la caméra 
	///////////////////////////////////////////////////////////////////
	void Camera::SetCameraCalibrationParameter(CvMat* intrinsecParameters, CvMat* distortionCoefficients)
	{
		if( (intrinsecParameters != NULL) && (distortionCoefficients != NULL) )
		{
			// Principal point
			cameraPrincipalPoint[0] = intrinsecParameters->data.fl[2];
			cameraPrincipalPoint[1] = intrinsecParameters->data.fl[5];
			
			// Dimension de la focale
			cameraFocalLength[0] = intrinsecParameters->data.fl[0];
			cameraFocalLength[1] = intrinsecParameters->data.fl[4];
			
			// Paramètres de distortion
			cameraDistortionCoefficients[0] = distortionCoefficients->data.fl[0];
			cameraDistortionCoefficients[1] = distortionCoefficients->data.fl[1];
			cameraDistortionCoefficients[2] = distortionCoefficients->data.fl[2];
			cameraDistortionCoefficients[3] = distortionCoefficients->data.fl[3];
		}						
	}
	
	///////////////////////////////////////////////////////////////////
	// Affecte les paramètres de positionnement de la caméra
	///////////////////////////////////////////////////////////////////
	void Camera::SetCameraPosition(const Cv3dTrackerCameraInfo & informations)
	{
		// Recopies de l'ensemble des informations de positionnement 
		// et de calibrage nécessaire au positionnement
		cameraInformations.principal_point.x = informations.principal_point.x;
		cameraInformations.principal_point.y = informations.principal_point.y;
		cameraInformations.valid = informations.valid;			
		
		for(int x=0; x<4; ++x)
		{
			for(int y=0; y<4; ++y)
			{
				cameraInformations.mat[x][y] = informations.mat[x][y];
			}
		}
	}
	
	///////////////////////////////////////////////////////////////////
	// Retourne la distance focale de la caméraS
	///////////////////////////////////////////////////////////////////
	float* Camera::GetFocalLength()
	{
		return cameraFocalLength;
	}
	
	///////////////////////////////////////////////////////////////////
	// Retourne les coordonnées du point principal
	///////////////////////////////////////////////////////////////////
	float* Camera::GetPrincipalPoint()
	{
		return cameraPrincipalPoint;
	}
	
	///////////////////////////////////////////////////////////////////
	// Retourne les coefficients de distosion de la caméra
	///////////////////////////////////////////////////////////////////
	float* Camera::GetDistortionCoefficients()
	{
		return cameraDistortionCoefficients;
	}
	
	///////////////////////////////////////////////////////////////////
	// Retourne les informations de positionnement de la caméra
	///////////////////////////////////////////////////////////////////
	Cv3dTrackerCameraInfo* Camera::GetCameraPosition()
	{
		return &cameraInformations;
	}
}
